#pragma once
#include <string>
#include <array>
#include <list>
#include <memory>
#include "zuc/nml_intf/zucpos2.h"
#include "axisgroup/fct_para_common.h"

namespace Util {
class IniParser;
}

namespace axisgroup {
class ConfigItem
{
public:
    ConfigItem(const char* path, void* value) : path_(path), value_(value) {}
    virtual void set_value(Util::IniParser* p) = 0;
    virtual void get_value(Util::IniParser* p) = 0;
    std::string path_;
    void* value_;
};

class AgConfig
{
public:
    struct Tcp1D
    {
        double trans;
        double ori;
    };
    struct Traj
    {
        Tcp1D vel;
        Tcp1D acc;
        Tcp1D jerk;
    };
    struct JointLimit
    {
        RobJointVal min;
        RobJointVal max;
        RobJointVal vel;
        RobJointVal acc;
        double jerk;
        RobJointVal err_limit;
    };
    struct Jog
    {
        Tcp1D vel;
        Tcp1D acc;
        Tcp1D jerk;
        RobJointVal j_vel;
        RobJointVal j_acc;
        double j_jerk;
        std::vector<double> pos_jog_acc;  // 步进JOG不同距离时的不同最大加速度
        std::vector<double> ori_jog_acc;  // 步进JOG不同距离时的不同最大加速度
    };
    struct Stop
    {
        Tcp1D cat1_acc;
        Tcp1D cat2_acc;
        double cat1_joint_acc;
        double cat2_joint_acc;
    };
    struct ServoMove
    {
        int filter_type;
        //mmf
        double mmf_max_buf;
        double mmf_kp;
        double mmf_kv;
        double mmf_ka;
        //nlf
        double nlf_vp;
        double nlf_ap;
        double nlf_jp;
        double nlf_vr;
        double nlf_ar;
        double nlf_jr;
        //lpf
        double lpf_cf;
    };
    struct DefaultJointPos
    {
        RobJointVal shrunken;   //收缩姿态，一般为打包时候的姿态
        RobJointVal stretched;  //展开姿态
        RobJointVal initial;    //初始姿态，一般默认为收缩姿态，也是手柄home键使用的姿态
        double tol;
        double vel_scale;
    };
    struct RobotAxisCfg
    {
        int axis_start_id = 0;
        std::string bus_chn;
        std::vector<int> bus_devid;
        int axis_num = 6;
    };
    struct RobotDefaultDH
    {
        RobotDH robot_dh;
        double dh[6 * MAX_AXIS];  // 机器人DH参数
    };
    struct RobotDefaultDYN
    {
        double dyn[14 * MAX_AXIS] = {};
        double link_dyn[14 * MAX_AXIS] = {};
        double base_dyn[14 * MAX_AXIS] = {};
        double dyn_traj_para[7 * MAX_AXIS * 4];
    };
    struct RobotJointPara  // 本结构体仅储存机型文件中的数据
    {
        RobJointVal offset;                   // 机器人相对与伺服关节的offset
        RobJointVal dir;                      // 机器人轴相对于伺服关节的方向
        RobJointVal gear_radio;               // 机器人减速机减速比
        RobJointVal motor_tor;                // 机器人电机额定力矩
        RobJointVal default_joint_min_pos;    // 机型硬限位
        RobJointVal default_joint_max_pos;    // 机型硬限位
        RobJointVal default_joint_vel_limit;  // 机型关节速度限制
        RobJointVal default_joint_acc_limit;  // 机型关节加速度限制
        RobJointVal default_joint_err_limit;  // 机型关节误差限制，给伺服用的
    };
    struct SafetyCollision
    {
        int collision_detection_enable;    // 0: 关闭；1： 开启
        int collision_method;              // 6: 关闭控制器观测
        int reaction_type;                 // 0: 停止; 1： 暂停
        double force_limit_value;          // 末端力限制
        double rebound_angle;              // 关节运动时主轴反弹角度
        double rebound_length;             // 直线运动时主轴反弹长度
        double rebound_j_acc;              // 反弹最大加速度
        double rebound_j_vel;              // 反弹最大速度
        double rebound_l_acc;              // 直线运动时主轴反弹最大加速度
        double rebound_l_vel;              // 直线运动时主轴反弹最大速度
        double gm_filter;                  // 广义滤波器
        int clsn_sensitivity;              // 伺服的力限制等级
        std::vector<double> gm_threshold;  // 不同等级下的阈值
        RobJointVal joint_sensitivity;     // 各关节的灵敏度
    };
    struct RobotDefaultSafety
    {
        double momentum_limit;
        double tcp_vel_limit;
        double robot_power_limit;
        double stopping_dist;
        double stopping_time;
    };
    struct RobotSafety
    {
        SafetyCollision collision;
    };
    struct RobotConfig
    {
        int32_t base_frame_id = 0;
        int32_t tool_frame_id = 0;
        int32_t user_frame_id = 0;
        int32_t work_payload_id = 0;    // 机器人末端的全部负载
        int32_t sensor_payload_id = 0;  // 机器人末端力传感器的负载
        FCAxisPara force_control;       // 力控相关参数
        struct Traj traj;
        struct JointLimit joint_limit;  // 配置参数
        struct Jog jog;
        struct Stop stop;
        struct ServoMove servo_move;
        struct DefaultJointPos default_pose;
        struct RobotAxisCfg robot_axis_cfg;
        RobotSafety robot_safety;  // 机器人安全参数，包含碰撞

        struct RobotModelData
        {
            double max_payload;                    // 机型最大负载质量
            RobotDefaultDH dh;                     // 机型默认运动学参数
            RobotDefaultDYN dyn;                   // 机型默认动力学参数
            RobotJointPara joint_para;             // 机型默认关节参数
            RobotDefaultSafety custom_setting[4];  // 机型文件中默认的快速设置参数
            struct Traj traj;                      // 机型文件中的笛卡尔运动参数
            struct DefaultJointPos default_pose;   // 机型文件中的默认姿态
        } robot_model;
    } config_;

    RobotConfig& get_robot_config();

public:
    AgConfig();
    virtual ~AgConfig() = default;
    //加载模型配置
    virtual int load_model_config(const std::string& file_name);
    //加载用户配置
    virtual int load_usr_config(std::string usr_cfg_file = "robconfig_0.ini");
    // 将序列号写入配置文件
    virtual void write_serial_num(const std::string& usr_cfg_file, int serial);
    // 将文件中读取的DH参数格式化
    void config_dh(int joint_num);
    // 用户配置中的部分值，需要根据机型文件中的值做限制
    virtual bool fit_to_robot_config();
    //写用户配置到文件
    virtual int write_usr_config_to_file();
    //复位所有配置
    virtual void reset();
    //通过序列号查找模型配置文件
    static std::string find_model_config(uint32_t serial_num);

    virtual int append_item(std::shared_ptr<ConfigItem> item);

    std::string usr_cfg_;                                 // 配置文件名
    std::list<std::shared_ptr<ConfigItem>> user_items_;   // 用户配置项
    std::list<std::shared_ptr<ConfigItem>> model_items_;  // 机型配置项，只读

    friend class AxisGroup;
    friend class Robot;
};
}  // namespace axisgroup
